﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace SharpSoft.Geometries.Kalman
{
    /// <summary>
    /// 卡尔曼滤波算法   坐标点速度用0.1s记，即:？/100ms
    /// </summary>
    public class KalmanFilter : IPointFilter
    {
        Kalman2D xk = new Kalman2D();
        Kalman2D yk = new Kalman2D();

        public KalmanFilter()
        {

        }
        /// <summary>
        /// 最小X值
        /// </summary>
        public double MinX { get; set; }
        /// <summary>
        /// 最小Y值
        /// </summary>
        public double MinY { get; set; }
        /// <summary>
        /// 最小速度
        /// </summary>
        public double MinVelocity { get; set; } = 0.1d;
        /// <summary>
        /// 协方差
        /// </summary>
        public double R { get; set; } = 5d;
        /// <summary>
        /// 方差
        /// </summary>
        public double PD { get; set; } = 10d;

        public void Reset(Point2D initPoint)
        {
            xk.Reset(MinX//X位置最小值
                , MinVelocity//速度最小值
                , R//协方差
                , PD//方差
                , initPoint.X//初始位置
                );
            yk.Reset(MinY//Y位置最小值
                , MinVelocity//速度最小值
                , R//协方差
                , PD//方差
                , initPoint.Y//初始位置
                );
        }
        Point2D? lastpoint = null;
        double lasttime = 0;
        public Point2D Update(Point2D p, double time)
        {
            double vx = 0d, vy = 0d;
            double timespan = time - lasttime;
            if (lastpoint.HasValue)
            {//计算速度
                vx = (p.X - lastpoint.Value.X) / timespan;
                vy = (p.Y - lastpoint.Value.Y) / timespan;
            }
            lastpoint = p;
            lasttime = time;
            var kx = xk.Update(p.X, vx, 1);
            var ky = yk.Update(p.Y, vy, 1);
            return new Point2D(kx, ky);
        }

    }
}
